import rclpy
from rclpy.node import Node
from tf2_ros import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
from tf_transformations import quaternion_from_euler
import math

class staticTFBroadcaster(Node):

    def __init__(self):
        super().__init__("static_tf_broadcaster")
        self.static_broadcaster=StaticTransformBroadcaster(self)
        self.pub_static_tf()

    def pub_static_tf(self):
        """
        发布静态tf
        """
        tranfrom =TransformStamped()
        tranfrom.header.frame_id ="base_link"
        tranfrom.child_frame_id="caream_link"
        tranfrom.header.stamp=self.get_clock().now().to_msg()

        tranfrom.transform.translation.x=0.5
        tranfrom.transform.translation.y=0.3
        tranfrom.transform.translation.z=0.6

        # 欧拉角转四元数 q=x,y,z,w
        q=quaternion_from_euler(math.radians(180),0,0)

        # 旋转部分进行赋值
        tranfrom.transform.rotation.x=q[0]
        tranfrom.transform.rotation.y=q[1]
        tranfrom.transform.rotation.z=q[2]
        tranfrom.transform.rotation.w=q[3]

        # 发布坐标
        self.static_broadcaster.sendTransform(tranfrom)
        self.get_logger().info(f"发布坐标：{tranfrom}")


def main():
    rclpy.init()
    node=staticTFBroadcaster()
    rclpy.spin(node)
    rclpy.shutdown()
    

